#include "FOC.h"

// 构造函数实现
FOCController::FOCController() : electricalAngle(0.0f) {}

// 更新电机角度
void FOCController::updateAngle(float angle) {
    electricalAngle = angle;

    // 确保角度在0~2π范围内
    while (electricalAngle > 2.0f * M_PI) electricalAngle -= 2.0f * M_PI;
    while (electricalAngle < 0) electricalAngle += 2.0f * M_PI;

    // 预计算正弦和余弦值
    sinTheta = std::sin(electricalAngle);
    cosTheta = std::cos(electricalAngle);
}

// 执行FOC算法核心部分
void FOCController::execute(float iu, float iv) {
    // 保存电流采样值
    this->iu = iu;
    this->iv = iv;

    // 计算第三相电流
    this->iw = -(iu + iv);

    // 执行变换
    clarkeTransform();
    parkTransform();
}

// 执行反Park变换
void FOCController::executeInverseTransform(float vd, float vq) {
    this->vd = vd;
    this->vq = vq;

    inverseParkTransform();
}

// 获取变换结果的实现
float FOCController::getIu() const { return iu; }
float FOCController::getIv() const { return iv; }
float FOCController::getIw() const { return iw; }

float FOCController::getIalpha() const { return ialpha; }
float FOCController::getIbeta() const { return ibeta; }

float FOCController::getId() const { return id; }
float FOCController::getIq() const { return iq; }

float FOCController::getValpha() const { return valpha; }
float FOCController::getVbeta() const { return vbeta; }

float FOCController::getElectricalAngle() const { return electricalAngle; }

// Clarke变换实现
void FOCController::clarkeTransform() {
    ialpha = iu;
    ibeta = (iu + 2 * iv) * 0.57735f;  // 1/sqrt(3)
}

// Park变换实现
void FOCController::parkTransform() {
    id = ialpha * cosTheta + ibeta * sinTheta;
    iq = -ialpha * sinTheta + ibeta * cosTheta;
}

// 反Park变换实现
void FOCController::inverseParkTransform() {
    valpha = vd * cosTheta - vq * sinTheta;
    vbeta = vd * sinTheta + vq * cosTheta;
}